import time
# daheng
import gxipy as gx
from PIL import Image
from datetime import datetime
import logging
import pandas as pd
import queue

import argparse
import os
import platform
import sys
from pathlib import Path
import socket
import torch
# yolo
from models.common import DetectMultiBackend
from utils.dataloaders import LoadImages
from utils.general import (LOGGER, Profile, check_img_size, colorstr, cv2,
                           increment_path, non_max_suppression, print_args, scale_boxes, strip_optimizer, xyxy2xywh)
from utils.plots import Annotator, colors, save_one_box
from utils.torch_utils import select_device, smart_inference_mode
import yaml
# pip install numpy
import numpy as np

from WorkBox import write_responseinfo
from WorkBox import ST
from WorkBox import send_data
from WorkBox import CheckClient
import copy
from WorkBox import print_log

pos_stack4 = 0
pos_stack5 = 0
result_queue = queue.Queue()
photo_queue = queue.Queue()
str_queue=queue.Queue()     

alter = '5000 00FF FF03 00 0E00 0000 0114 0100'
check = '5000 00FF FF03 00 0C00 0000 0104 0100'
soft_component_decimal = list(range(500, 741, 4))
soft_component_hex = [hex(num)[2:].upper() for num in soft_component_decimal]
soft_component = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in soft_component_hex]

directory_path = 'E:\\new_images\\batch'

# 枚举设备。dev_info_list 是设备信息列表，列表的元素个数为枚举到的设备个数，列表元素是字
# 典，其中包含设备索引（index）、ip 信息（ip）等设备信息
device_manager = gx.DeviceManager()
dev_num, dev_info_list = device_manager.update_device_list()
if dev_num == 0:
    print('相机未连接')
    sys.exit(1)
# 打开设备
# 获取设备基本信息列表
strSN = dev_info_list[0].get("sn")
# 通过序列号打开设备
cam = device_manager.open_device_by_sn(strSN)
print_log("打开设备成功")

def alter_component(component_n, zero_one):
    # 修改plc中软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2000
    component_list = list(range(500, 773, 4))
    component_hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_component_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in component_hex_list]  # 修改三位数的格式为 'BC0A00'
    if zero_one == 0:
        command_str = alter + new_component_list[component_n] + '90 0400' + '0000'
    elif zero_one == 1:
        command_str = alter + new_component_list[component_n] + '90 0400' + '1000'
    else:
        print('软元件只能修改为0或1，请输入int类型')
        return False
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    time.sleep(0.1)
    if received == b'\xd0\x00\x00\xff\xff\x03\x00\x02\x00\x00\x00':
        return True
    else:
        return False


def check_component(component_n):
    # 查找n号软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2000
    component_list = list(range(500, 773, 4))
    hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_hex_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in hex_list]  # 修改三位数的格式为 'BC0A00'
    command_str = check + new_hex_list[component_n] + '90 0400'
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    result = ''.join(format(byte, '02X') for byte in received)
    time.sleep(0.1)
    return result[-4]


def alter_component2(component_n, zero_one):
    # 修改plc中软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2001
    component_list = list(range(500, 773, 4))
    component_hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_component_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in component_hex_list]  # 修改三位数的格式为 'BC0A00'
    if zero_one == 0:
        command_str = alter + new_component_list[component_n] + '90 0400' + '0000'
    elif zero_one == 1:
        command_str = alter + new_component_list[component_n] + '90 0400' + '1000'
    else:
        print('软元件只能修改为0或1，请输入int类型')
        return False
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    time.sleep(0.1)
    if received == b'\xd0\x00\x00\xff\xff\x03\x00\x02\x00\x00\x00':
        return True
    else:
        return False


def check_component2(component_n):
    # 查找n号软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2001
    component_list = list(range(500, 773, 4))
    hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_hex_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in hex_list]  # 修改三位数的格式为 'BC0A00'
    command_str = check + new_hex_list[component_n] + '90 0400'
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    result = ''.join(format(byte, '02X') for byte in received)
    time.sleep(0.1)
    return result[-4]


def alter_component3(component_n, zero_one):
    # 修改plc中软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2002
    component_list = list(range(500, 773, 4))
    component_hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_component_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in component_hex_list]  # 修改三位数的格式为 'BC0A00'
    if zero_one == 0:
        command_str = alter + new_component_list[component_n] + '90 0400' + '0000'
    elif zero_one == 1:
        command_str = alter + new_component_list[component_n] + '90 0400' + '1000'
    else:
        print('软元件只能修改为0或1，请输入int类型')
        return False
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    time.sleep(0.1)
    if received == b'\xd0\x00\x00\xff\xff\x03\x00\x02\x00\x00\x00':
        return True
    else:
        return False


def check_component3(component_n):
    # 查找n号软元件信息
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    Host = '192.168.1.150'
    port = 2002
    component_list = list(range(500, 773, 4))
    hex_list = [hex(num)[2:].upper() for num in component_list]  # 转换为十六进制并大写
    new_hex_list = [hex_str[1:] + '0' + hex_str[0] + '00' for hex_str in hex_list]  # 修改三位数的格式为 'BC0A00'
    command_str = check + new_hex_list[component_n] + '90 0400'
    command_str_msg = bytes.fromhex(command_str)
    connected = False
    received = None
    while not connected:
        try:
            sock.connect((Host, port))
            sock.send(command_str_msg)
            received = sock.recv(1024)
            connected = True
        except ConnectionRefusedError:
            time.sleep(0.1)
    sock.close()
    result = ''.join(format(byte, '02X') for byte in received)
    time.sleep(0.1)
    return result[-4]


# 启动plc
# alter_component(64, 1)
def robot_one():
    process_round_r1 = 0
    def robot1_func1(func1_round):
        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))

            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(0.1)

            message = '26,set_var,int-instructio#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False

        HOST = '192.168.1.100'
        PORT = 8011
        s11 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s11.bind((HOST, PORT))

        # 交替移动到1/2号垛
        s11.listen()
        conn1, addr1 = s11.accept()
        if conn1.recv(1024):
            if func1_round % 2 == 0:
                alter_component(10, 0)
                alter_component(65, 0)
                #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
                alter_component(10, 1)
                alter_component(65, 1)
                time.sleep(1)
                while check_component(18) == '0':
                    time.sleep(1)
                alter_component(18, 1)

            else:
                alter_component(11, 0)
                alter_component(65, 0)
                #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
                alter_component(11, 1)
                alter_component(65, 1)
                time.sleep(1)
                while check_component(19) == '0':
                    time.sleep(1)
                alter_component(19, 1)

            conn1.sendall('go'.encode())
            time.sleep(1)

        # 移动到一号交互位
        s11.listen()
        conn1, addr1 = s11.accept()
        if conn1.recv(1024):
            alter_component(15, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(15, 1)
            alter_component(65, 1)
            while check_component(23) == '0':
                time.sleep(1)
            alter_component(23, 0)
            conn1.sendall('go'.encode())

        # 查询高低信息
        s11.listen()
        conn1, addr1 = s11.accept()
        if conn1.recv(1024):
            while check_component(29) == '0' and check_component(30) == '0':
                time.sleep(2)
            if check_component(29) == '1':  # 查询为高
                message = '23,set_var,int-height1#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(30) == '1':  # 查询为低
                message = '23,set_var,int-height1#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                #self.backgroundThread_Bot1.log_message('查询交互1高低信息失败')
                return False
            conn1.sendall('go'.encode('utf-8'))
        s11.listen()
        conn1, addr1 = s11.accept()
        if conn1.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(0.1)
            alter_component(26, 0)
            conn1.sendall('go'.encode('utf-8'))
        s11.close()
        server_sock.close()

    def robot1_func2():
        try:
            PT = pd.read_excel('PT_Robot_One.xls', sheet_name='dataGridView_P')
        except FileNotFoundError:
            #self.backgroundThread_Bot1.log_message('机械臂点位文件缺失，请检查')
            return False
        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))
            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(1)

            message = '26,set_var,int-instructio#2\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False

        HOST = '192.168.1.100'
        PORT = 8012
        s12 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s12.bind((HOST, PORT))
        s12.listen()
        conn12, addr12 = s12.accept()
        if conn12.recv(1024):
            alter_component(15, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(15, 1)
            alter_component(65, 1)
            time.sleep(1)
            while check_component(23) == '0':
                time.sleep(1)
            alter_component(23, 0)

            conn12.sendall('go'.encode())
            time.sleep(2)

        # 查询高低信息
        s12.listen()
        conn12, addr12 = s12.accept()
        if conn12.recv(1024):
            while check_component(29) == '0' and check_component(30) == '0':
                time.sleep(2)
            if check_component(29) == '1':  # 查询为高
                message = '23,set_var,int-height1#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(30) == '1':  # 查询为低
                message = '23,set_var,int-height1#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                #self.backgroundThread_Bot1.log_message('查询交互1高低信息失败')
                return False
            conn12.sendall('go'.encode('utf-8'))

        s12.listen()
        conn12, addr12 = s12.accept()
        if conn12.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(1)
            alter_component(26, 0)
            conn12.sendall('go'.encode('utf-8'))

        s12.listen()
        conn12, addr12 = s12.accept()
        if conn12.recv(1024):
            alter_component(12, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(12, 1)
            alter_component(65, 1)
            time.sleep(1)
            while check_component(20) == '0':
                time.sleep(1)
            alter_component(20, 0)

            conn12.sendall('go'.encode())

        s12.listen()
        conn12, addr12 = s12.accept()
        pos = '1'
        if conn12.recv(1024):
            full = True
            while full:
                if check_component(7) == '1':
                    # 报警
                    print('报警')
                    time.sleep(2)
                else:
                    full = False

            if check_component(2) == '1':
                pos = '2'
            if check_component(3) == '1':
                pos = '3'
            if check_component(4) == '1':
                pos = '4'
            if check_component(5) == '1':
                pos = '5'
            if check_component(6) == '1':
                pos = '6'
            original = ['-1'] * 5
            result_df = PT[PT['名称'] == pos]
            for index, row in result_df.iterrows():
                original[0] = str(row['X'])
                original[1] = str(row['Y'])
                original[2] = str(row['Z'])
                original[3] = str(row['R'])
                original[4] = str(row['手系'])
            reply = ','.join(original).encode()
            conn12.sendall(reply)
        s12.close()
        server_sock.close()

    def robot1_func3():
        try:
            PT = pd.read_excel('PT_Robot_One.xls', sheet_name='dataGridView_P')
        except FileNotFoundError:
            #self.backgroundThread_Bot1.log_message('机械臂点位文件缺失，请检查')
            return False

        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))
            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(0.1)

            message = '26,set_var,int-instructio#3\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False

        HOST = '192.168.1.100'
        PORT = 8013
        s13 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s13.bind((HOST, PORT))
        s13.listen()
        conn13, addr13 = s13.accept()
        if conn13.recv(1024):
            alter_component(12, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(12, 1)
            alter_component(65, 1)
            conn13.sendall('go'.encode())
            time.sleep(1)
            while check_component(20) == '0':
                time.sleep(1)
            alter_component(20, 0)

        s13.listen()
        conn13, addr13 = s13.accept()
        pos = ''
        if conn13.recv(1024):
            empty = True
            while empty:
                if check_component(2) == '0':
                    # 报警
                    print('报警')
                    time.sleep(2)
                else:
                    empty = False

            if check_component(2) == '1':
                pos = '1'
            if check_component(3) == '1':
                pos = '2'
            if check_component(4) == '1':
                pos = '3'
            if check_component(5) == '1':
                pos = '4'
            if check_component(6) == '1':
                pos = '5'
            if check_component(7) == '1':
                pos = '6'
            original = [''] * 5
            result_df = PT[PT['名称'] == pos]
            for index, row in result_df.iterrows():
                original[0] = str(row['X'])
                original[1] = str(row['Y'])
                original[2] = str(row['Z'])
                original[3] = str(row['R'])
                original[4] = str(row['手系'])
            reply = ','.join(original).encode()
            conn13.sendall(reply)  ######## todo

        s13.listen()
        conn13, addr13 = s13.accept()
        if conn13.recv(1024):
            alter_component(16, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(16, 1)
            alter_component(65, 1)

            while check_component(24) == '0':
                time.sleep(1)
            alter_component(24, 0)

            conn13.sendall('go'.encode())

        s13.listen()
        conn1, addr1 = s13.accept()
        if conn1.recv(1024):
            while check_component(35) == '0' and check_component(36) == '0':
                time.sleep(2)
            if check_component(35) == '1':  # 查询为高
                message = '23,set_var,int-height2#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(36) == '1':  # 查询为低
                message = '23,set_var,int-height2#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                #self.backgroundThread_Bot1.log_message('查询交互2高低信息失败')
                return False
            conn1.sendall('go'.encode('utf-8'))

        s13.listen()
        conn13, addr13 = s13.accept()
        if conn13.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(1)
            alter_component(26, 0)
            conn13.sendall('go'.encode('utf-8'))

        s13.close()
        server_sock.close()

    def robot1_func4():
        try:
            PT = pd.read_excel('PT_Robot_One.xls', sheet_name='dataGridView_P')
        except FileNotFoundError:
            #self.backgroundThread_Bot1.log_message('机械臂点位文件缺失，请检查')
            return False
        global pos_stack4
        while pos_stack4 == 6:
            if check_component(8) == '0':
                pos_stack4 = 0
            else:
                time.sleep(2)
                #self.backgroundThread_Bot1.log_message('中转区托盘满，请拿走一部分空托盘留出位置')
                # TODO 报警

        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))

            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(1)

            message = '26,set_var,int-instructio#4\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False

        HOST = '192.168.1.100'
        PORT = 8014
        s14 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s14.bind((HOST, PORT))
        s14.listen()
        conn14, addr14 = s14.accept()
        if conn14.recv(1024):
            alter_component(16, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(16, 1)
            alter_component(65, 1)
            time.sleep(1)
            while check_component(24) == '0':
                time.sleep(1)
            alter_component(24, 0)
            conn14.sendall('go'.encode())
            time.sleep(1)

        s14.listen()
        conn1, addr1 = s14.accept()
        if conn1.recv(1024):
            while check_component(35) == '0' and check_component(36) == '0':
                time.sleep(2)
            if check_component(35) == '1':  # 查询为高
                message = '23,set_var,int-height2#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(36) == '1':  # 查询为低
                message = '23,set_var,int-height2#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                #self.backgroundThread_Bot1.log_message('查询交互2高低信息失败')
                return False
            conn1.sendall('go'.encode('utf-8'))

        s14.listen()
        conn14, addr14 = s14.accept()
        if conn14.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(1)
            alter_component(26, 0)
            conn14.sendall('go'.encode('utf-8'))

        s14.listen()
        conn14, addr14 = s14.accept()
        if conn14.recv(1024):
            alter_component(13, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(13, 1)
            alter_component(65, 1)
            time.sleep(1)
            while check_component(21) == '0':
                time.sleep(1)
            alter_component(21, 0)
            conn14.sendall('go'.encode())
            time.sleep(1)

        s14.listen()
        conn14, addr14 = s14.accept()
        original = ['-1'] * 5
        result_df = PT[PT['名称'] == str(pos_stack4 + 1)]
        for index, row in result_df.iterrows():
            original[0] = str(row['X'])
            original[1] = str(row['Y'])
            original[2] = str(row['Z'])
            original[3] = str(row['R'])
            original[4] = str(row['手系'])
        reply = ','.join(original).encode()
        conn14.sendall(reply)
        pos_stack4 += 1
        s14.close()
        server_sock.close()

    def robot1_func5():
        try:
            PT = pd.read_excel('PT_Robot_One.xls', sheet_name='dataGridView_P')
        except FileNotFoundError:
            #self.backgroundThread_Bot1.log_message('机械臂点位文件缺失，请检查')
            return False
        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))

            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(1)

            message = '26,set_var,int-instructio#5\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            #self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False

        HOST = '192.168.1.100'
        PORT = 8015
        s15 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s15.bind((HOST, PORT))

        s15.listen()
        conn15, addr15 = s15.accept()
        if conn15.recv(1024):
            alter_component(12, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(12, 1)
            alter_component(65, 1)
            time.sleep(1)
            while check_component(20) == '0':
                time.sleep(1)
            alter_component(20, 0)
            conn15.sendall('go'.encode())
            time.sleep(1)

        s15.listen()
        conn15, addr15 = s15.accept()
        pos = ''
        if conn15.recv(1024):
            empty = True
            while empty:
                if check_component(2) == '0':
                    # 报警
                    print('报警')
                    time.sleep(5)
                else:
                    empty = False

            if check_component(2) == '1':
                pos = '1'
            if check_component(3) == '1':
                pos = '2'
            if check_component(4) == '1':
                pos = '3'
            if check_component(5) == '1':
                pos = '4'
            if check_component(6) == '1':
                pos = '5'
            if check_component(7) == '1':
                pos = '6'
            original = ['-1'] * 5
            result_df = PT[PT['名称'] == pos]
            for index, row in result_df.iterrows():
                original[0] = str(row['X'])
                original[1] = str(row['Y'])
                original[2] = str(row['Z'])
                original[3] = str(row['R'])
                original[4] = str(row['手系'])
            reply = ','.join(original).encode()
            conn15.sendall(reply)

        s15.listen()
        conn15, addr15 = s15.accept()
        if conn15.recv(1024):
            alter_component(17, 0)
            alter_component(65, 0)
            #self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(17, 1)
            alter_component(65, 1)
            while check_component(25) == '0':
                time.sleep(1)
            alter_component(25, 0)
            conn15.sendall('go'.encode())
            time.sleep(1)

        s15.listen()
        conn15, addr15 = s15.accept()
        if conn15.recv(1024):
            while check_component(41) == '0' and check_component(42) == '0':
                time.sleep(5)
            if check_component(41) == '1':  # 查询为高
                message = '23,set_var,int-height3#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(42) == '1':  # 查询为低
                message = '23,set_var,int-height3#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                #self.backgroundThread_Bot1.log_message('查询交互1高低信息失败')
                return False
            conn15.sendall('go'.encode('utf-8'))

        s15.listen()
        conn15, addr15 = s15.accept()
        if conn15.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(1)
            alter_component(26, 0)
            conn15.sendall('go'.encode('utf-8'))

        s15.close()
        server_sock.close()

    def robot1_func6():
        try:
            PT = pd.read_excel('PT_Robot_One.xls', sheet_name='dataGridView_P')
        except FileNotFoundError:
            # self.backgroundThread_Bot1.log_message('机械臂点位文件缺失，请检查')
            return False
        Server_HOST = '127.0.0.1'
        Server_PORT = 1001
        server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            server_sock.connect((Server_HOST, Server_PORT))

            message = '16,get_var,int-go\r\n'
            task_finished = False
            while not task_finished:
                server_sock.send(message.encode('utf-8'))
                message_received = server_sock.recv(1024).decode()
                if message_received == '18,get_var,int-go#0\r\n':
                    task_finished = True
                else:
                    print('等待动作完成')
                    time.sleep(1)

            message = '26,set_var,int-instructio#6\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                # self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
            message = '18,set_var,int-go#1\r\n'
            server_sock.send(message.encode('utf-8'))
            received_message = server_sock.recv(1024).decode()
            if received_message == message:
                pass
            else:
                # self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
                return False
        except ConnectionRefusedError:
            # self.backgroundThread_Bot1.log_message('变量服务器连接失败，请检查机械臂1及其变量服务器是否正常打开')
            return False
        global pos_stack5
        HOST = '192.168.1.100'
        PORT = 8016
        s16 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s16.bind((HOST, PORT))
        s16.listen()
        conn16, addr16 = s16.accept()
        if conn16.recv(1024):
            alter_component(17, 0)
            alter_component(65, 0)
            # self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(17, 1)
            alter_component(65, 1)
            while check_component(25) == '0':
                time.sleep(1)
            alter_component(25, 0)
            conn16.sendall('go'.encode())
            time.sleep(1)

        s16.listen()
        conn16, addr16 = s16.accept()
        if conn16.recv(1024):
            while check_component(41) == '0' and check_component(42) == '0':
                time.sleep(5)
            if check_component(41) == '1':  # 查询为高
                message = '23,set_var,int-height3#2\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    # self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            elif check_component(42) == '1':  # 查询为低
                message = '23,set_var,int-height3#1\r\n'
                server_sock.send(message.encode('utf-8'))
                received_message = server_sock.recv(1024).decode()
                if received_message == message:
                    pass
                else:
                    #self.backgroundThread_Bot1.log_message('变量服务器连接失败或变量更改有误，请检查机械臂1及其变量服务器是否正常打开')
                    return False
            else:
                # self.backgroundThread_Bot1.log_message('查询交互1高低信息失败')
                return False
            conn16.sendall('go'.encode('utf-8'))

        s16.listen()
        conn16, addr16 = s16.accept()
        if conn16.recv(1024):  # 通知plc机械臂1动作完成
            alter_component(26, 1)
            time.sleep(1)
            alter_component(26, 0)
            conn16.sendall('go'.encode('utf-8'))

        s16.listen()
        conn16, addr16 = s16.accept()
        if conn16.recv(1024):
            alter_component(14, 0)
            alter_component(65, 0)
            # self.backgroundThread_Bot1.log_message('正在给电机发送指令')
            alter_component(14, 1)
            alter_component(65, 1)
            while check_component(22) == '0':
                time.sleep(1)
            alter_component(22, 0)
            conn16.sendall('go'.encode())
            time.sleep(1)

        while pos_stack5 == 6:
            if check_component(9) == '0':
                pos_stack5 = 0
            else:
                time.sleep(1)
                # self.backgroundThread_Bot1.log_message('五号垛满，请取走托盘')
                # TODO 报警
        s16.listen()
        conn16, addr16 = s16.accept()
        original = ['-1'] * 5
        result_df = PT[PT['名称'] == str(pos_stack5 + 1)]
        for index, row in result_df.iterrows():
            original[0] = str(row['X'])
            original[1] = str(row['Y'])
            original[2] = str(row['Z'])
            original[3] = str(row['R'])
            original[4] = str(row['手系'])
        reply = ','.join(original).encode()
        conn16.sendall(reply)
        pos_stack5 += 1

        s16.close()
        server_sock.close()

    # global flag
    # if not flag:
    #     self.backgroundThread_Bot1.log_message('尚未自检，机械臂1无法启动，请先自检')
    #     return False
    # self.backgroundThread_Bot1.log_message('机械臂1打开')
    priority_queue = queue.Queue()
    while ST["MechanicalArm_One"]:
        if priority_queue.empty():
            if check_component(28) == '1':
                priority_queue.put(2)
                alter_component(28, 0)
            if check_component(27) == '1':
                priority_queue.put(1)
                alter_component(27, 0)
            if check_component(40) == '1':
                priority_queue.put(6)
                alter_component(40, 0)
            if check_component(39) == '1':
                priority_queue.put(5)
                alter_component(39, 0)
            if check_component(34) == '1':
                priority_queue.put(4)
                alter_component(34, 0)
            if check_component(33) == '1':
                priority_queue.put(3)
                alter_component(33, 0)
            time.sleep(1)
        else:
            direction = priority_queue.get()
            if direction == 1:
                while check_component(0) == '0' or check_component(1) == '0':
                    # TODO 报警
                    # self.backgroundThread_Bot1.log_message('1号或2号垛上料位未检测到托盘，请放入托盘')
                    time.sleep(1)
                robot1_func1(process_round_r1)
                process_round_r1 += 1
            elif direction == 2:
                robot1_func2()
            elif direction == 3:
                robot1_func3()
            elif direction == 4:
                if check_component(8) == '0':
                    global pos_stack4
                    pos_stack4 = 0
                robot1_func4()
            elif direction == 5:
                robot1_func5()
            elif direction == 6:
                if check_component(9) == '0':
                    global pos_stack5
                    pos_stack5 = 0
                robot1_func6()

def robot_two():
    # global flag
    # while not flag:
    #     self.backgroundThread_Bot2.log_message('尚未自检，机械臂2无法启动程序，请先自检')
    #     return False
    # self.backgroundThread_Bot2.log_message('机械臂2打开')
    try:
        PT = pd.read_excel('PT_Robot_Two.xls', sheet_name='dataGridView_P')
    except FileNotFoundError:
        # self.backgroundThread_Bot2.log_message('机械臂2点位文件缺失，请检查')
        return False
    process_round = 6
    while ST["MechanicalArm_Two"]:
        HOST = '192.168.1.100'
        PORT21 = 8021
        s21 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s21.bind((HOST, PORT21))
        s21.listen()

        # 抓取镜片
        conn21, addr21 = s21.accept()
        received = conn21.recv(1024)
        alter_component2(48, 1)
        pos_num = process_round % 12 + 1
        if pos_num <= 6:
            pos_up = str(pos_num) + '_up'
        else:
            pos_up = str(pos_num - 6) + '_up'
        original = ['-1'] * 11
        result_df = PT[PT['名称'] == pos_up]
        for index, row in result_df.iterrows():
            original[0] = str(row['X'])
            original[1] = str(row['Y'])
            original[2] = str(row['Z'])
            original[3] = str(row['R'])
            original[4] = str(row['手系'])
        result_df = PT[PT['名称'] == str(pos_num)]
        for index, row in result_df.iterrows():
            original[5] = str(row['X'])
            original[6] = str(row['Y'])
            original[7] = str(row['Z'])
            original[8] = str(row['R'])
            original[9] = str(row['手系'])
        original[10] = str(pos_num)
        if pos_num <= 6:
            while check_component2(31) == '0':
                time.sleep(2)

        elif pos_num >= 7:
            while check_component2(32) == '0':
                time.sleep(2)

        if received.decode() == '01':
            print('received')
            print('数字:', pos_num, pos_up, original)
            reply = ','.join(original).encode()
            conn21.sendall(reply)
            print('sent')
            s21.close()

        # 判断模组安全
        PORT22 = 8022
        s22 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s22.bind((HOST, PORT22))
        s22.listen()
        conn22, addr22 = s22.accept()
        received = conn22.recv(1024)
        alter_component2(48, 1)
        if (process_round + 1) % 6 == 0:
            alter_component2(45, 1)

        empty = False
        while not empty:
            if check_component2(53) == '1' or check_component2(54) == '1' or check_component2(
                    55) == '1' or check_component2(56) == '1':
                time.sleep(2)
            else:
                empty = True

        secure = False
        while not secure:
            if check_component2(50) == '1':
                secure = True
                alter_component2(48, 0)
            else:
                time.sleep(1)

        message2 = 'go'
        if received:
            conn22.sendall(message2.encode())
            print('sent')
            s22.close()

        # 放镜片动作完成
        PORT23 = 8023
        s23 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s23.bind((HOST, PORT23))
        s23.listen()

        conn23, addr23 = s23.accept()
        conn23.recv(1024)
        alter_component2(48, 1)
        alter_component2(68, 0)
        s23.close()
        process_round += 1

def robot_three(client_socket):
    # global flag
    # while not flag:
    #     self.backgroundThread_Bot3.log_message('尚未自检，机械臂3无法启动程序，请先自检')
    #     return False

    # 读取 Excel 文件
    try:
        PT = pd.read_excel('PT_Robot_Three.xls', sheet_name='dataGridView_P')
    except FileNotFoundError:
        # self.backgroundThread_Bot3.log_message('机械臂3点位文件缺失，请检查')
        return False
    # self.backgroundThread_Bot3.log_message('机械臂3打开')
    connected = False
    ok_round = 6
    ng_tray_count = 1
    tray_ng = [[0] * 6 for _ in range(4)]  # 记录托盘信息

    HOST_ROBOT3 = '127.0.0.1'
    PORT_ROBOT3 = 1003
    s_bot = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

    HOST = '192.168.1.100'
    PORT31 = 8031
    s31 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s31.bind((HOST, PORT31))
    PORT32 = 8032
    s32 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s32.bind((HOST, PORT32))
    PORT33 = 8033
    s33 = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    s33.bind((HOST, PORT33))
    while not connected:
        try:
            s_bot.connect((HOST_ROBOT3, PORT_ROBOT3))
            logging.info('正在开始机械臂3程序')
            # self.backgroundThread_Bot3.log_message('程序初始化')
            message = '21,set_var,int-start#2\r\n'
            s_bot.send(message.encode('utf-8'))
            message1 = s_bot.recv(1024)
            if message1.decode('utf-8') == message:
                # self.backgroundThread_Bot3.log_message('初始化成功，机械臂3正在等待指令')
                connected = True
                time.sleep(2)
                alter_component3(49, 1)
            else:
                # self.backgroundThread_Bot3.log_message('初始化失败，请检查机械臂3服务器是否正常')
                pass
        except ConnectionRefusedError:
            # self.backgroundThread_Bot3.log_message('机械臂3连接失败，正在重试，请检查机械臂3是否打开')
            time.sleep(5)
    while ST["MechanicalArm_Three"]:
        original = ['-1'] * 25
        while check_component3(51) == '0' or check_component3(52) == '0':
            time.sleep(1)
            print_log('等待模组到位')
        if check_component3(57) == '0' or check_component3(58) == '0' or check_component3(59) == '0' or check_component3(60) == '0':
            while result_queue.qsize() < 4:
                # self.backgroundThread_Bot3.log_message('等待分析')
                time.sleep(1)
            for i in range(0, 4):
                original[i] = result_queue.get()
            # self.backgroundThread_Bot3.error_result('传感器未检测到镜片，跳过此批')
            #传感器为检测到照片
            #========================================================================
            temp=[]
            if not str_queue.empty():
                temp=str_queue.get()     #取出队列中的数组结果
            else:
                print_log("str_queue没有数据!!!!!")
            #先发送一个心跳包给客户端   等待响应
            if CheckClient(client_socket):
                args=["Error"]
                data=write_responseinfo("glass_process_data",args,temp)       #检测出错
                send_data(client_socket,data)
            #========================================================================
        else:
            s31.listen()
            conn31, addr31 = s31.accept()
            conn31.recv(1024)
            alter_component3(49, 0)
            conn31.sendall('go'.encode())
            conn31.recv(1024)
            alter_component3(49, 1)

            while result_queue.qsize() < 4:
                #self.backgroundThread_Bot3.log_message('等待分析')
                time.sleep(1)
            for i in range(0, 4):
                original[i] = result_queue.get()

            if original[0] != '0' and original[1] != '0' and original[2] != '0' and original[3] != '0':
                all_one = True
            else:
                all_one = False
            if all_one:
                tray_ok_final = -1
            else:
                tray_ok_final = ok_round % 12
                ok_round += 1

            tray_ok_final += 1
            target_name_ok = str(tray_ok_final)
            if tray_ok_final <= 6:
                target_name_ok_up = str(tray_ok_final) + '_' + 'up'
            else:
                target_name_ok_up = str(tray_ok_final - 6) + '_' + 'up'

            if target_name_ok == '-1':
                for _ in range(4, 14):
                    original[_] = '0'
            else:
                result_df = PT[PT['名称'] == target_name_ok]  ###################################
                for index, row in result_df.iterrows():
                    original[9] = str(row['X'])
                    original[10] = str(row['Y'])
                    original[11] = str(row['Z'])
                    original[12] = str(row['R'])
                    original[13] = str(row['手系'])
                result_df = PT[PT['名称'] == target_name_ok_up]  ###################################
                for index, row in result_df.iterrows():
                    original[4] = str(row['X'])
                    original[5] = str(row['Y'])
                    original[6] = str(row['Z'])
                    original[7] = str(row['R'])
                    original[8] = str(row['手系'])

            # 方案2 放镜片时从某一个高度下落，均见缝插针（会有下落声音）
            # col_ok = [5]*4
            # col_ok_final = 0
            # for i in range(0, 3):
            #     if original2[i] == 0:
            #         for j in range(5, 0, -1):
            #             if tray_ok[i][j] == 0 and j <= col_ok[i]:
            #                 col_ok[i] = j
            # for i in range(0, 3):
            #     if col_ok_final < col_ok[i]:
            #         col_ok_final = col_ok[i]
            # for i in range(0, 3):
            #     if original2[i] == 0:
            #         tray_ok[i][col_ok_final] = 1
            # if col_ok_final == 5:
            #     tray_ok = [[0] * 6 for _ in range(4)]
            # original2[4] = col_ok_final

            col_ng_final = 0
            if original[0] == '0' and original[1] == '0' and original[2] == '0' and original[3] == '0':
                all_zero = True
            else:
                all_zero = False

            if not all_zero:
                for i1 in range(5, -1, -1):
                    ok_flag = [False, False, False, False]
                    for j in range(0, 4):
                        if original[j] == '0':
                            ok_flag[j] = True
                        elif original[j] != '0':
                            if tray_ng[j][i1] == 0:
                                ok_flag[j] = True
                    if ok_flag == [True, True, True, True]:
                        col_ng_final = i1
                for k in range(0, 4):
                    if original[k] != '0':
                        tray_ng[k][col_ng_final] = 1

                col_ng_final += 13
                target_name_ng_up = str(col_ng_final) + '_' + 'up'
                if ng_tray_count % 2 == 1:
                    col_ng_final += 6
                if col_ng_final == 18 or col_ng_final == 24:
                    ng_tray_count += 1
                    tray_ng = [[0] * 6 for _ in range(4)]
                target_name_ng = str(col_ng_final)
                original[24] = target_name_ng_up

                result_df = PT[PT['名称'] == target_name_ng]
                for index, row in result_df.iterrows():
                    original[19] = str(row['X'])
                    original[20] = str(row['Y'])
                    original[21] = str(row['Z'])
                    original[22] = str(row['R'])
                    original[23] = str(row['手系'])
                result_df = PT[PT['名称'] == target_name_ng_up]
                for index, row in result_df.iterrows():
                    original[14] = str(row['X'])
                    original[15] = str(row['Y'])
                    original[16] = str(row['Z'])
                    original[17] = str(row['R'])
                    original[18] = str(row['手系'])
            elif all_zero:
                for _ in range(14, 25):
                    original[_] = '0'
            reply = ','.join(original).encode()
            print(f'发送信息为：ok:{tray_ok_final},ng:{col_ng_final},{reply}')
            #===============================================================
            tempdata=[]
            if not str_queue.empty():
                tempdata=str_queue.get()     #取出队列中的数组结果
            else:
                print_log("str_queue没有数据!!!!!")
            #先发送一个心跳包给客户端   等待响应  将检测结果发给客户端
            if CheckClient(client_socket):
                args=["OK",tray_ok_final,col_ng_final]
                data=write_responseinfo("glass_process_data",args,tempdata)
                send_data(client_socket,data)
            #===============================================================


            if 6 >= tray_ok_final >= 1:
                while check_component3(43) == '0':
                    time.sleep(2)
                    # self.backgroundThread.log_message('等待ok上托盘到位')
            if 12 >= tray_ok_final >= 7:
                while check_component3(44) == '0':
                    time.sleep(2)
                    # self.backgroundThread.log_message('等待ok下托盘到位')
            if 13 <= col_ng_final <= 18:
                while check_component3(37) == '0':
                    time.sleep(2)
                    # self.backgroundThread.log_message('等待ng上托盘到位')
            if 24 >= col_ng_final >= 19:
                while check_component3(38) == '0':
                    time.sleep(2)
                    # self.backgroundThread.log_message('等待ng下托盘到位')

            # 方案3  TODO 结合方案1和2，每次先判断能否插位置，当a边可以插b边不可以时，先放b边（防止镜片碰撞）；如果两边都可以，优先放ok片

            s32.listen()
            conn32, addr32 = s32.accept()
            conn32.recv(1024)
            conn32.sendall(reply)  # 发送给机械臂
            logging.info('分类信息已发送给机械臂，等待下一批')
            # self.backgroundThread.log_message('分类信息已发送给机械臂，等待下一批\n等待PLC信号\n\n')

            conn33, addr33 = s33.accept()
            conn33.recv(1024)
            if tray_ok_final == 6 or tray_ok_final == 12:
                # print('ok要托盘')
                alter_component3(47, 1)
            if col_ng_final == 18 or col_ng_final == 24:
                # print('ng要托盘')
                alter_component3(46, 1)
            conn33.sendall('go'.encode())

filenamequeue=queue.Queue()      #存储拍照图片的路径
import threading
def photo_shoot():    #拍照  和保存图片 和plc 沟通
    def photo(photo_dir, photo_num):
        # 开始采集
        cam.stream_on()
        # 从第 0 个流通道获取一幅图像
        raw_image = cam.data_stream[0].get_image()
        # 从 RAW 图像数据创建 numpy 数组
        numpy_image = raw_image.get_numpy_array()
        image = Image.fromarray(numpy_image, 'L')
        timestamp = datetime.now()
        file_name = timestamp.strftime('%Y%m%d_%H%M%S.bmp')
        image.save(os.path.join(photo_dir, f'{file_name}'))
        with open(os.path.join(photo_dir, f'{file_name}'), 'rb') as f:
            image_data = f.read()
        # self.backgroundThread_PhotoShoot.log_image(QByteArray(image_data))
        # self.backgroundThread_PhotoShoot.image_name_message(f'{file_name}')
        logging.info(f'成功保存第{photo_num}张相片')
        # self.backgroundThread_PhotoShoot.log_message(f'成功保存第{photo_num}张相片')
        photo_queue.put(os.path.join(photo_dir, f'{file_name}'))
        cam.stream_off()
        filenamequeue.put(file_name)     #将图片路径 添加到队列中
        

    # self.backgroundThread_PhotoShoot.log_message('开始拍照')
    HOST = '192.168.1.100'
    PORT_PHOTO = 8001   # PLC->相机
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    # s.setblocking(False)
    s.bind((HOST, PORT_PHOTO))
    # TODO multiprocessing.Manager 共享 socket 对象
    s.listen()
    s.settimeout(0.1)
    while ST["photo_shoot"]:
        try:
            conn,addr=s.accept()
            conn.settimeout(0.1)
            while ST["photo_shoot"]:
                print_log("photo_shoot current thread_id:" + threading.current_thread().name)
                pictures_batch_dir = increment_path(directory_path)
                os.makedirs(pictures_batch_dir, exist_ok=True)
                # ================================================
                while True:
                    if ST["photo_shoot"]:
                        try:
                            request = conn.recv(1024)
                            if  request:
                                break
                            else:
                                pass
                        except socket.timeout:
                            pass
                    else:
                        print_log("====s.close========")
                        s.close()
                        print_log("====s.close=====")
                        return
                # ================================================
                request = int(request.decode().encode('unicode_escape').decode('utf-8').replace('\\x', ''))
                # request = int(request.decode().encode('unicode_escape').decode('utf-8').replace('\\x', ''))
                # self.backgroundThread_PhotoShoot.log_message(f'正在拍第{request}张镜片')
                # 对plc发来信号进行解码
                if request == 1:
                    original = '1'
                elif request == 2:
                    original = '22'
                elif request == 3:
                    original = '333'
                elif request == 4:
                    original = '4444'
                # 发回给plc的信息，说明已拍照成功
                photo(pictures_batch_dir, request)
                conn.sendall(original.encode())
        except socket.timeout:
            pass
    print_log("====s.close========")
    s.close()
    print_log("====s.close=====")
def glass_process(client_socket):        #分析检查  照片分析结果      
    
    #处理图片  返回缺陷路径 和 标注图片路径
    print_log("glass_process 执行中")
    def yolo_process(source_dir):
        FILE = Path(__file__).resolve()  # detect.py绝对路径
        ROOT = FILE.parents[0]  # YOLOv5 root directory
        if str(ROOT) not in sys.path:  # 模块的查询路径
            sys.path.append(str(ROOT))  # add ROOT to PATH
        ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # relative

        def process(
                weights=ROOT / 'yolov5s.pt',  # model path or triton URL
                data=ROOT / 'data/coco128.yaml',  # dataset.yaml path
                imgsz=(640, 640),  # inference size (height, width)
                conf_thres=0.25,  # confidence threshold
                iou_thres=0.45,  # NMS IOU threshold
                max_det=1000,  # maximum detections per image
                device='0',  # cuda device, i.e. 0 or 0,1,2,3 or cpu
                view_img=True,  # show results
                save_txt=True,  # save results to *.txt
                save_conf=True,  # save confidences in --save-txt labels
                save_crop=True,  # save cropped prediction boxes
                nosave=False,  # do not save images/videos
                classes=None,  # filter by class: --class 0, or --class 0 2 3
                agnostic_nms=False,  # class-agnostic NMS
                augment=False,  # augmented inference
                visualize=False,  # visualize features
                update=False,  # update all models
                project='E:/detect_images',  # save results to project/name
                name='exp',  # save results to project/name
                exist_ok=False,  # existing project/name ok, do not increment
                line_thickness=1,  # bounding box thickness (pixels)
                hide_labels=False,  # hide labels
                hide_conf=False,  # hide confidences
                half=False,  # use FP16 half-precision inference
                dnn=False,  # use OpenCV DNN for ONNX inference
                vid_stride=1,  # video frame-rate stride
        ):
            save_img = not nosave
            # Load model
            # self.backgroundThread_GlassProcess.log_message('正在加载网络结构')
            device = select_device(device)
            model = DetectMultiBackend(weights, device=device, dnn=dnn, data=data, fp16=half)
            # model.to(device)? todo
            stride, names, pt = model.stride, model.names, model.pt
            imgsz = check_img_size(imgsz, s=stride)  # check image size

            source = source_dir
            # Directories
            save_dir = increment_path(Path(project) / name, exist_ok=exist_ok)  # increment run
            (save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True)  # make dir

            # Dataloader
            bs = 1  # batch_size
            dataset = LoadImages(source, img_size=imgsz, stride=stride, auto=pt, vid_stride=vid_stride)
            vid_path, vid_writer = [None] * bs, [None] * bs

            # Run inference
            model.warmup(imgsz=(1 if pt or model.triton else bs, 3, *imgsz))  # warmup
            seen, windows, dt = 0, [], (Profile(), Profile(), Profile())
            for path, im, im0s, vid_cap, s in dataset:
                with dt[0]:
                    im = torch.from_numpy(im).to(model.device)
                    im = im.half() if model.fp16 else im.float()  # uint8 to fp16/32
                    im /= 255  # 0 - 255 to 0.0 - 1.0
                    if len(im.shape) == 3:
                        im = im[None]  # expand for batch dim

                # Inference
                with dt[1]:
                    visualize = increment_path(save_dir / Path(path).stem,
                                                mkdir=True) if visualize else False
                    pred = model(im, augment=augment, visualize=visualize)

                # NMS
                with dt[2]:
                    pred = non_max_suppression(pred, conf_thres, iou_thres, classes, agnostic_nms,
                                                max_det=max_det)

                # Second-stage classifier (optional)
                # pred = utils.general.apply_classifier(pred, classifier_model, im, im0s)

                # Process predictions
                for i, det in enumerate(pred):  # per image
                    seen += 1
                    p, im0, frame = path, im0s.copy(), getattr(dataset, 'frame', 0)

                    p = Path(p)  # to Path
                    save_path = str(save_dir / p.name)  # im.jpg
                    txt_path = str(save_dir / 'labels' / p.stem) + (
                        '' if dataset.mode == 'image' else f'_{frame}')  # im.txt
                    s += '%gx%g ' % im.shape[2:]  # print string
                    gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
                    imc = im0.copy() if save_crop else im0  # for save_crop
                    annotator = Annotator(im0, line_width=line_thickness, example=str(names))
                    if len(det):
                        # Rescale boxes from img_size to im0 size
                        det[:, :4] = scale_boxes(im.shape[2:], det[:, :4], im0.shape).round()

                        # Print results
                        for c in det[:, 5].unique():
                            n = (det[:, 5] == c).sum()  # detections per class
                            s += f"{n} {names[int(c)]}{'s' * (n > 1)}, "  # add to string

                        # Write results
                        for *xyxy, conf, cls in reversed(det):
                            if save_txt:  # Write to file
                                xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(
                                    -1).tolist()  # normalized xywh
                                line = (cls, *xywh, conf) if save_conf else (cls, *xywh)  # label format
                                with open(f'{txt_path}.txt', 'a') as f:
                                    f.write(('%g ' * len(line)).rstrip() % line + '\n')

                            if save_img or save_crop or view_img:  # Add bbox to image
                                c = int(cls)  # integer class
                                label = None if hide_labels else (
                                    names[c] if hide_conf else f'{names[c]} {conf:.2f}')
                                annotator.box_label(xyxy, label, color=colors(c, True))
                            if save_crop:
                                save_one_box(xyxy, imc,
                                                file=save_dir / 'crops' / names[c] / f'{p.stem}.jpg',
                                                BGR=True)

                    # Stream results
                    im0 = annotator.result()
                    if view_img:
                        if platform.system() == 'Linux' and p not in windows:
                            windows.append(p)
                            cv2.namedWindow(str(p),
                                            cv2.WINDOW_NORMAL | cv2.WINDOW_KEEPRATIO)  # allow window resize (Linux)
                            cv2.resizeWindow(str(p), im0.shape[1], im0.shape[0])
                        cv2.imshow(str(p), im0)
                        cv2.waitKey(1)  # 1 millisecond

                    # Save results (image with detections)
                    if save_img:
                        if dataset.mode == 'image':
                            cv2.imwrite(save_path, im0)  #图片保存位置
                        else:  # 'video' or 'stream'
                            if vid_path[i] != save_path:  # new video
                                vid_path[i] = save_path
                                if isinstance(vid_writer[i], cv2.VideoWriter):
                                    vid_writer[i].release()  # release previous video writer
                                if vid_cap:  # video
                                    fps = vid_cap.get(cv2.CAP_PROP_FPS)
                                    w = int(vid_cap.get(cv2.CAP_PROP_FRAME_WIDTH))
                                    h = int(vid_cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
                                else:  # stream
                                    fps, w, h = 30, im0.shape[1], im0.shape[0]
                                save_path = str(
                                    Path(save_path).with_suffix(
                                        '.mp4'))  # force *.mp4 suffix on results videos
                                vid_writer[i] = cv2.VideoWriter(save_path, cv2.VideoWriter_fourcc(*'mp4v'),
                                                                fps,
                                                                (w, h))
                            vid_writer[i].write(im0)

                # Print time (inference-only)
                LOGGER.info(f"{s}{'' if len(det) else '(no detections), '}{dt[1].dt * 1E3:.1f}ms")

                # Print results
                t = tuple(x.t / seen * 1E3 for x in dt)  # speeds per image
                LOGGER.info(
                    f'Speed: %.1fms pre-process, %.1fms inference, %.1fms NMS per image at shape {(1, 3, *imgsz)}' % t)
                if save_txt or save_img:  
                    s = f"\n{len(list(save_dir.glob('labels/*.txt')))} labels saved to {save_dir / 'labels'}" if save_txt else ''
                    LOGGER.info(f"Results saved to {colorstr('bold', save_dir)}{s}")
                if update:
                    strip_optimizer(weights[0])  # update model (to fix SourceChangeWarning)
            return str(os.path.abspath(txt_path)), save_path  #缺陷路径 检测完后图片路径

        def parse_opt():
            parser = argparse.ArgumentParser()
            # parser.add_argument('--weights', nargs='+', type=str, default=ROOT / 'yolov5s.pt', help='model path or triton URL')
            # parser.add_argument('--weights', nargs='+', type=str, default=r'.\runs\train\exp35\weights\best.pt', help='model path or triton URL')
            parser.add_argument('--weights', nargs='+', type=str, default=r'.\runs\train\exp2\weights\last.pt',
                                help='model path or triton URL')
            # parser.add_argument('--source', type=str, default=r'E:\new_images',
            #                     help='file/dir/URL/glob/screen/0(webcam)')
            # parser.add_argument('--source', type=str, default=r'.\datasets\0701_A\images\train', help='file/dir/URL/glob/screen/0(webcam)')
            # parser.add_argument('--source', type=str, default=r'C:\Users\bird\Downloads\data1\images', help='file/dir/URL/glob/screen/0(webcam)')
            # parser.add_argument('--data', type=str, default=ROOT / 'data/coco128.yaml', help='(optional) dataset.yaml path')
            # parser.add_argument('--data', type=str, default=r'D:\yolov5-master\data\g0528_1.yaml', help='(optional) dataset.yaml path')
            parser.add_argument('--imgsz', '--img', '--img-size', nargs='+', type=int, default=[4096],
                                help='inference size h,w')
            parser.add_argument('--conf-thres', type=float, default=0.1,
                                help='confidence threshold')  # default 0.05
            parser.add_argument('--iou-thres', type=float, default=0.1, help='NMS IoU threshold')
            parser.add_argument('--max-det', type=int, default=1000, help='maximum detections per image')
            parser.add_argument('--device', default='0', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
            parser.add_argument('--view-img', action='store_true', help='show results')
            parser.add_argument('--save-txt', default=True, action='store_true', help='save results to *.txt')
            parser.add_argument('--save-conf', default=True, action='store_true',
                                help='save confidences in --save-txt labels')
            parser.add_argument('--save-crop', action='store_true', help='save cropped prediction boxes')
            parser.add_argument('--nosave', action='store_true', help='do not save images/videos')
            parser.add_argument('--classes', nargs='+', type=int,
                                help='filter by class: --classes 0, or --classes 0 2 3')
            parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
            parser.add_argument('--augment', action='store_true', help='augmented inference')
            parser.add_argument('--visualize', action='store_true', help='visualize features')
            parser.add_argument('--update', action='store_true', help='update all models')
            parser.add_argument('--project', default='E:/detect_images', help='save results to project/name')
            parser.add_argument('--name', default='exp', help='save results to project/name')
            parser.add_argument('--exist-ok', action='store_true',
                                help='existing project/name ok, do not increment')
            parser.add_argument('--line-thickness', default=1, type=int, help='bounding box thickness (pixels)')
            parser.add_argument('--hide-labels', default=False, action='store_true', help='hide labels')
            parser.add_argument('--hide-conf', default=False, action='store_true', help='hide confidences')
            parser.add_argument('--half', action='store_true', help='use FP16 half-precision inference')
            parser.add_argument('--dnn', action='store_true', help='use OpenCV DNN for ONNX inference')
            parser.add_argument('--vid-stride', type=int, default=1, help='video frame-rate stride')
            opt = parser.parse_args()
            opt.imgsz *= 2 if len(opt.imgsz) == 1 else 1  # expand
            print_args(vars(opt))
            return opt

        opt = parse_opt()
        return process(**vars(opt))
    
    #返回镜片圆心位置以及半径
    def edge_detect(source):
        if os.path.isfile(source):  # 确定路径是文件
            # 读取图像
            image = cv2.imread(source, 0)  # 以灰度图像方式读取
            # 图像预处理
            blur = cv2.GaussianBlur(image, (5, 5), 0)
            # 进行圆检测
            image_height, image_width = image.shape[:2]
            min_radius = int(image_width * 0.75 / 2)  # 最小半径
            max_radius = int(image_width * 0.8 / 2)  # 最大半径
            circles = cv2.HoughCircles(blur, cv2.HOUGH_GRADIENT, dp=1, minDist=image_width, param1=50,
                                        param2=30, minRadius=min_radius, maxRadius=max_radius)
            # 确保至少检测到一个圆
            if circles is not None:
                # 取第一个检测到的圆
                detected_circle = np.round(circles[0, :]).astype(int)
                # 遍历检测到的圆
                for (x0, y0, r0) in detected_circle:
                    # 写入结果到文件中
                    x1 = x0 / 5120
                    y1 = y0 / 5120
                    r1 = r0 / 5120
                    logging.info('拍到一张镜片，等待分析')
                    return x1, y1, r1
            else:
                logging.warning('未拍到镜片，请检查')
                return 0, 0, 0
        else:
            logging.error('文件保存路径有误')
        
        #检查镜片是好是坏
    def picture_classify(flaw_file, edge_x, edge_y, edge_r):
        if edge_r == 0:
            print('未检测到镜片，分类结果为0')
            return '-1'
        with open('D:/yolov5-master/opt-config/standard.yaml', 'r') as file:
            config = yaml.safe_load(file)
        R = int(config.get('Standard')['R'])
        area1 = float(config.get('Standard')['area1'])
        area2 = float(config.get('Standard')['area2'])
        area3 = 1
        flaws_sum = int(config.get('Standard')['sum'])
        count, count11, count12, count21, count22, count31, count32, count33, count34 = 0, 0, 0, 0, 0, 0, 0, 0, 0
        try:
            with open(f'{flaw_file}.txt', 'r') as f:
                lines = f.readlines()
        except FileNotFoundError:
            return '0'
        for line in lines:
            _, flaw_x, flaw_y, a1, a2, _ = line.split()
            a1 = float(a1)
            a2 = float(a2)
            distance = ((float(flaw_x) - edge_x) ** 2 + (float(flaw_y) - edge_y) ** 2) ** 0.5
            ratio = R / (edge_r * 2)
            if max(a1, a2) > 7 / ratio:
                print('有缺陷尺寸大于7mm，分类结果为1')
                return '1'
            elif max(a1, a2) > 5 / ratio:
                if distance < edge_r * area2:
                    print('B区内有缺陷大于5mm，分类结果为1')
                    return '1'
                else:
                    count34 += 1
                    count += 1
            elif max(a1, a2) > 0.5 / ratio:
                if distance < edge_r * area1:
                    count12 += 1
                    count += 1
                elif distance < edge_r * area2:
                    count22 += 1
                    count += 1
                else:
                    count33 += 1
                    count += 1
            elif max(a1, a2) > 0.25 / ratio:
                if distance < edge_r * area1:
                    count12 += 1
                    count += 1
                elif distance < edge_r * area2:
                    count22 += 1
                    count += 1
                else:
                    count32 += 1
                    count += 1
            elif max(a1, a2) > 0.15 / ratio:
                if distance < edge_r * area1:
                    count12 += 1
                    count += 1
                elif distance < edge_r * area2:
                    count22 += 1
                    count += 1
                else:
                    count31 += 1
                    count += 1
            elif max(a1, a2) > 0.1 / ratio:
                if distance < edge_r * area1:
                    count12 += 1
                    count += 1
                elif distance < edge_r * area2:
                    count21 += 1
                    count += 1
                else:
                    count31 += 1
                    count += 1
            else:
                if distance < edge_r * area1:
                    count11 += 1
                    count += 1
                elif distance < edge_r * area2:
                    count21 += 1
                    count += 1
                else:
                    count31 += 1
                    count += 1
            if count > flaws_sum:
                print('总缺陷数高于阈值，分类结果为1')
                return '1'

        if count11 <= 10 and count12 <= 2 and count21 <= 13 and count22 <= 2 and count31 <= 10 and count32 <= 2 and count33 <= 1 and count34 <= 2 and count <= flaws_sum:
            print('符合条件，分类结果为0')
            return '0'
        else:
            print('区域内缺陷数过多，分类结果为1')
            return '1'
    
    print_log("glass_process")
    photoinfo={"original_image_url":"",     #存储图片的信息
             "detection_image_url":"",
             "flaw_document_url":"",
             "is_valid":"",
             "x":"",
             "y":"",
             "r":"",
             "time":"",
             "index":""}  
    photot_arr =[] #存储图片数据
    #存储图片数组  四张图片为一组
    i = 0
    while ST["glass_process"]:
        global photo_queue
        while photo_queue.empty() :                                                          #picture_location 原图路径
            if not ST["glass_process"]:
                print_log("推出 glass_process 函数")
                return
            time.sleep(1)                                                                   #flaws 缺陷文档路径
        picture_location = photo_queue.get()         #原照片的路径                           #check_picture 检测完后图片路径
        flaws, check_picture = yolo_process(picture_location)                               #result 检测结果是否有缺陷  0有 1没有
        x, y, r = edge_detect(picture_location)    #边缘检查                                 #x,y,r  镜片圆心位置以及半径  
        result = picture_classify(flaws, x, y, r)  #照片分类
        global result_queue
        result_queue.put(result)

        photoinfo1=copy.deepcopy(photoinfo)
        photoinfo1["original_image_url"]=picture_location
        photoinfo1["detection_image_url"]=check_picture
        photoinfo1["flaw_document_url"]=flaws
        photoinfo1["is_valid"]=result
        photoinfo1["x"]=x
        photoinfo1["y"]=y
        photoinfo1["r"]=r
        if not filenamequeue.empty():
            photoinfo1["time"] = filenamequeue.get()
        photoinfo1['index']=str(i%4)
        i+=1
        photot_arr.append(photoinfo1)
        print_log("当前队列中photoinfo 数量："+str(len(photot_arr)))
        #判断当前数组是否满四个 满四个 将photo_arr 数组添加到队列中
        if len(photot_arr)==4:
           str_queue.put(photot_arr[:])   #浅拷贝
           print_log("====================")
           print_log(photot_arr[0])
           print_log(photot_arr[1])
           print_log(photot_arr[2])
           print_log(photot_arr[3])
           print_log("=======================")
           photot_arr.clear()
    print_log("推出 glass_process 函数")
           